import tkinter as tk
import sim
import rotate
from tkinter import simpledialog
import CalAngel

#这个文件用于仿照tk_move_class.py文件
#大体是一致的，但是这里会有两个joint。分别是joint1和joint2

class five_axis:
    def __init__(self,root):
        self.root=root
        self.joint_name2='joint2'
        self.joint_name1='joint1'

        self.x_positive=False
        self.x_negative=False
        self.y_positive=False
        self.y_negative=False
        self.z_positive=False
        self.z_negative=False

        self.joint1_rotate_left=False
        self.joint1_rotate_right=False

        self.joint2_rotate_left=False
        self.joint2_rotate_right=False

        self.clientID = self.connect_to_coppeliasim()

        self.positive_y_button = tk.Button(root, text="Y Moveforward",width=15,height=2)
        #self.positive_y_button.place(x=0,y=0)
        self.positive_y_button.pack()
        self.positive_y_button.bind("<ButtonPress-1>", self.y_positive_press)
        self.positive_y_button.bind("<ButtonRelease-1>", self.y_positive_release)

        self.negative_y_button = tk.Button(root, text="Y Movebackward",width=15,height=2)
        #self.negative_y_button.place(x=0,y=60)
        self.negative_y_button.pack()
        self.negative_y_button.bind("<ButtonPress-1>", self.y_negative_press)
        self.negative_y_button.bind("<ButtonRelease-1>", self.y_negative_release)

        self.positive_x_button = tk.Button(root, text="X Moveforward",width=15,height=2)
        self.positive_x_button.place(x=0,y=0)
        #self.positive_x_button.pack()
        self.positive_x_button.bind("<ButtonPress-1>", self.x_positive_press)
        self.positive_x_button.bind("<ButtonRelease-1>", self.x_positive_release)

        self.negative_x_button = tk.Button(root, text="X Movebackward",width=15,height=2)
        self.negative_x_button.place(x=0,y=60)
        #self.negative_x_button.pack()
        self.negative_x_button.bind("<ButtonPress-1>", self.x_negative_press)
        self.negative_x_button.bind("<ButtonRelease-1>", self.x_negative_release)

        self.positive_z_button = tk.Button(root, text="Z Moveforward",width=15,height=2)
        self.positive_z_button.pack()
        self.positive_z_button.bind("<ButtonPress-1>", self.z_positive_press)
        self.positive_z_button.bind("<ButtonRelease-1>", self.z_positive_release)

        self.negative_z_button = tk.Button(root, text="Z Movebackward",width=15,height=2)
        self.negative_z_button.pack()
        self.negative_z_button.bind("<ButtonPress-1>", self.z_negative_press)
        self.negative_z_button.bind("<ButtonRelease-1>", self.z_negative_release)

        self.joint1_rotate_left=tk.Button(root, text="Joint1 Rotate Left",width=15,height=2)
        self.joint1_rotate_left.place(x=0,y=120)
        #self.joint1_rotate_left.pack()
        self.joint1_rotate_left.bind("<ButtonPress-1>", self.joint1_rotate_left_press)
        self.joint1_rotate_left.bind("<ButtonRelease-1>", self.joint1_rotate_left_release)

        self.joint1_rotate_right=tk.Button(root, text="Joint1 Rotate Right",width=15,height=2)
        #self.joint1_rotate_right.pack()
        self.joint1_rotate_right.place(x=0,y=180)
        self.joint1_rotate_right.bind("<ButtonPress-1>", self.joint1_rotate_right_press)
        self.joint1_rotate_right.bind("<ButtonRelease-1>", self.joint1_rotate_right_release)

        self.joint2_rotate_left=tk.Button(root, text="Joint2 Rotate Left",width=15,height=2)
        #self.joint2_rotate_left.place(x=0,y=120)
        self.joint2_rotate_left.pack()
        self.joint2_rotate_left.bind("<ButtonPress-1>", self.joint2_rotate_left_press)
        self.joint2_rotate_left.bind("<ButtonRelease-1>", self.joint2_rotate_left_release)

        self.joint2_rotate_right=tk.Button(root, text="Joint2 Rotate Right",width=15,height=2)
        #self.joint2_rotate_right.place(x=0,y=180)
        self.joint2_rotate_right.pack()
        self.joint2_rotate_right.bind("<ButtonPress-1>", self.joint2_rotate_right_press)
        self.joint2_rotate_right.bind("<ButtonRelease-1>", self.joint2_rotate_right_release)

        self.rotate_with_button1=tk.Button(root, text="Rotate_joint1",command=self.button_click1,width=15,height=2)
        self.rotate_with_button1.place(x=0,y=240)
        #self.rotate_with_button1.pack()

        self.rotate_with_button2=tk.Button(root, text="Rotate_joint2",command=self.button_click2,width=15,height=2)
        #self.rotate_with_button2.place(x=0,y=240)
        self.rotate_with_button2.pack()



    def connect_to_coppeliasim(self,port=19999):
        sim.simxFinish(-1)  # 关闭所有连接
        clientID = sim.simxStart('127.0.0.0', port, True, True, 5000, 5)
        if clientID!= -1:
            print('Connected to CoppeliaSim')
        else:
            raise Exception('Failed to connect to CoppeliaSim')
        return clientID
    
    def move_object(self, clientID, object_name, velocity, axis, direction):
        _, handle = sim.simxGetObjectHandle(clientID, object_name, sim.simx_opmode_blocking)
        if handle!= -1:
            _, position = sim.simxGetObjectPosition(clientID, handle, -1, sim.simx_opmode_blocking)
            new_position = list(position)
            if axis == 'y':
                new_position[1] += velocity if direction == 'positive' else -velocity
            elif axis == 'z':
                new_position[2] += velocity if direction == 'positive' else -velocity
            elif axis == 'x':
                new_position[0] += velocity if direction == 'positive' else -velocity
            sim.simxSetObjectPosition(clientID, handle, -1, new_position, sim.simx_opmode_oneshot)

    def x_positive_press(self,event):
        global x_positive
        x_positive = True
        self.action_x_positive()

    def x_positive_release(self,event):
        global x_positive
        x_positive = False

    def x_negative_press(self,event):
        global x_negative
        x_negative = True
        self.action_x_negative()

    def x_negative_release(self,event):
        global x_negative
        x_negative = False

    def y_positive_press(self,event):
        global y_positive
        y_positive = True
        self.action_y_positive()
    
    def y_positive_release(self,event):
        global y_positive
        y_positive = False
    
    def y_negative_press(self,event):
        global y_negative
        y_negative = True
        self.action_y_negative()
    
    def y_negative_release(self,event):
        global y_negative
        y_negative = False
    
    def z_positive_press(self,event):
        global z_positive
        z_positive = True
        self.action_z_positive()

    def z_positive_release(self,event):
        global z_positive
        z_positive = False

    def z_negative_press(self,event):
        global z_negative
        z_negative = True
        self.action_z_negative()

    def z_negative_release(self,event):
        global z_negative
        z_negative = False

    def joint1_rotate_left_press(self,event):
        global joint1_rotate_left
        joint1_rotate_left = True
        self.action_joint1_rotate_left()

    def joint1_rotate_left_release(self,event):
        global joint1_rotate_left
        joint1_rotate_left = False

    def joint1_rotate_right_press(self,event):
        global joint1_rotate_right
        joint1_rotate_right = True
        self.action_joint1_rotate_right()

    def joint1_rotate_right_release(self,event):
        global joint1_rotate_right
        joint1_rotate_right = False

    def joint2_rotate_left_press(self,event):
        global joint2_rotate_left
        joint2_rotate_left = True
        self.action_joint2_rotate_left()

    def joint2_rotate_left_release(self,event):
        global joint2_rotate_left
        joint2_rotate_left = False

    def joint2_rotate_right_press(self,event):
        global joint2_rotate_right
        joint2_rotate_right = True
        self.action_joint2_rotate_right()

    def joint2_rotate_right_release(self,event):
        global joint2_rotate_right
        joint2_rotate_right = False

    def action_joint1_rotate_left(self,event):
        if joint1_rotate_left:
            self.rotate_joint1_left_speed()
            self.root.after(100, self.action_joint1_rotate_left)

    def action_joint1_rotate_right(self,event):
        if joint1_rotate_right:
            self.rotate_joint1_right_speed()
            self.root.after(100, self.action_joint1_rotate_right)

    def action_joint2_rotate_left(self,event):
        if joint2_rotate_left:
            self.rotate_joint2_left_speed()
            self.root.after(100, self.action_joint2_rotate_left)

    def action_joint2_rotate_right(self,event):
        if joint2_rotate_right:
            self.rotate_joint2_right_speed()
            self.root.after(100, self.action_joint2_rotate_right)
    
    def action_x_positive(self):
        if x_positive:
            self.move_object(self.clientID, self.joint_name1, 0.001, 'x', 'positive')
            self.root.after(100, self.action_x_positive)
    
    def action_x_negative(self):
        if x_negative:
            self.move_object(self.clientID, self.joint_name1, 0.001, 'x', 'negative')
            self.root.after(100, self.action_x_negative)

    def action_y_positive(self):
        if y_positive:
            self.move_object(self.clientID, self.joint_name2, 0.001, 'y', 'positive')
            self.root.after(100, self.action_y_positive)

    def action_y_negative(self):
        if y_negative:
            self.move_object(self.clientID, self.joint_name2, 0.001, 'y', 'negative')
            self.root.after(100, self.action_y_negative)

    def action_z_positive(self):
        if z_positive:
            self.move_object(self.clientID, self.joint_name2, 0.001, 'z', 'positive')
            self.root.after(100, self.action_z_positive)

    def action_z_negative(self):
        if z_negative:
            self.move_object(self.clientID, self.joint_name2, 0.001, 'z', 'negative')
            self.root.after(100, self.action_z_negative)

    def rotate_joint1_left_speed(self):
        current_angle=CalAngel.get_joint_angle(self.clientID, self.joint_name1)
        if current_angle>-90:
            self.rotate_both(current_angle-1,self.joint_name1)
            current_angle=current_angle-1

    def rotate_joint1_right_speed(self):
        current_angle=CalAngel.get_joint_angle(self.clientID, self.joint_name1)
        if current_angle<90:
            self.rotate_both(current_angle+1,self.joint_name1)
            current_angle=current_angle+1

    def rotate_joint2_left_speed(self):
        current_angle=CalAngel.get_joint_angle(self.clientID, self.joint_name2)
        if current_angle>-90:
            self.rotate_both(current_angle-1,self.joint_name2)
            current_angle=current_angle-1


    def rotate_joint2_right_speed(self):
        current_angle=CalAngel.get_joint_angle(self.clientID, self.joint_name2)
        if current_angle<90:
            self.rotate_both(current_angle+1,self.joint_name2)
            current_angle=current_angle+1

    def rotate_both(self,angle,name):
        rotate.rotate_joint_speed(self.clientID, name, angle,1)

    def button_click1(self):
        angle=simpledialog.askfloat("Input", "Please input the angle",parent=self.root)
        if angle is not None:
            self.rotate_both(angle,self.joint_name1)
    def button_click2(self):
        angle=simpledialog.askfloat("Input", "Please input the angle",parent=self.root)
        if angle is not None:
            self.rotate_both(angle,self.joint_name2)
if __name__ == "__main__":
    root=tk.Tk()
    root.title("Move Object")
    root.geometry("800x600")
    five_axis(root)
    root.mainloop()
